#include "DofSplineCatmullRom.h"

// mCtrlPts of DofAbstract should not be used except in the constructor!!!! 
// only spline mCtrlPts used !

DofSplineCatmullRom::DofSplineCatmullRom(int _index){
	mStartIndex=_index;
	mNumPts=0;
	mSplineCR=NULL;
	mFrameOffset=-1;
}

DofSplineCatmullRom::DofSplineCatmullRom(int _index, double nfstart, double nfend, int npts){
	mStartIndex=_index;
	mFrameOffset = nfstart;

	// compute the ctrl pts
	double diff = (double)(nfend-nfstart)/(npts-1);
	vector<Vec2d> ctrlPts;
	ctrlPts.clear();
	ctrlPts.resize(npts);
	for(int i=0; i<npts; i++) {
		ctrlPts[i][0]=nfstart+i*diff;
		ctrlPts[i][1]=0;//(double)rand()/(RAND_MAX+1);
	}
	mNumPts=npts;
	
	// just to remind that padding is done so all indices have to be offset accordingly
	mSplineCR = new SplineCatmullRom(ctrlPts);
}

DofSplineCatmullRom::DofSplineCatmullRom(int _index, vector<Vec2d> _ctrlPts){
	mStartIndex=_index;
	mFrameOffset = _ctrlPts[0][0];

	mNumPts=_ctrlPts.size();

	// just to remind that padding is done so all indices have to be offset accordingly
	mSplineCR = new SplineCatmullRom(_ctrlPts);
}

double DofSplineCatmullRom::getValue(double nFrame){
	return mSplineCR->getValue(nFrame);
}

double DofSplineCatmullRom::getSlope(double _frame){
	return mSplineCR->getSlope(_frame);
}

double DofSplineCatmullRom::getSecondSlope(double _frame){
	return mSplineCR->getSecondSlope(_frame);
}

double DofSplineCatmullRom::getCtrlPtCoord(int _numC){
	return mSplineCR->getCtrlPt(_numC + 1)[0];
}

double DofSplineCatmullRom::getCtrlPtValue(int _numC){
	return mSplineCR->getCtrlPt(_numC + 1)[1];
}

void DofSplineCatmullRom::setCtrlPtValue(int _numC, double _v){
	mSplineCR->getCtrlPt(_numC + 1)[1] = _v;
	if(_numC+1==1){
		mSplineCR->getCtrlPt(0)[1] = mSplineCR->getCtrlPt(1)[1];
	}
	if(_numC+1==mNumPts){
		mSplineCR->getCtrlPt(mNumPts+1)[1] = mSplineCR->getCtrlPt(mNumPts)[1];
	}
}

void DofSplineCatmullRom::getCtrlPts(double *state){
	for(int i=0; i<mNumPts; i++){
		state[mStartIndex+i] = mSplineCR->getCtrlPt(i+1)[1];	// mCtrlPts offset by 1
	}
}

vector<Vec2d> DofSplineCatmullRom::getCtrlPts(){
	vector<Vec2d> ctrlPts;
	for(int i=0; i<mNumPts; i++){
		ctrlPts.push_back(mSplineCR->getCtrlPt(i+1));	// mCtrlPts offset by 1
	}
	return ctrlPts;
}

void DofSplineCatmullRom::getCtrlPts(double *state, int _start, int _end){
	for(int i=_start; i<=_end; i++){
		state[mStartIndex+i] = mSplineCR->getCtrlPt(i+1)[1];	// mCtrlPts offset by 1
	}
}

void DofSplineCatmullRom::setCtrlPts(double *state){
	for(int i=0; i<mNumPts; i++){
		mSplineCR->getCtrlPt(i+1)[1] = state[mStartIndex+i];	// mCtrlPts offset by 1
	}
	mSplineCR->getCtrlPt(0)[1] = mSplineCR->getCtrlPt(1)[1];
	mSplineCR->getCtrlPt(mNumPts+1)[1] = mSplineCR->getCtrlPt(mNumPts)[1];
}

void DofSplineCatmullRom::setCtrlPts(double *state, int _start, int _end){
	for(int i=_start; i<=_end; i++){
		mSplineCR->getCtrlPt(i+1)[1] = state[mStartIndex+i];	// mCtrlPts offset by 1
	}
	if(_start==0) mSplineCR->getCtrlPt(0)[1] = mSplineCR->getCtrlPt(1)[1];
	if(_end==mNumPts-1) mSplineCR->getCtrlPt(mNumPts+1)[1] = mSplineCR->getCtrlPt(mNumPts)[1];
}

void DofSplineCatmullRom::addCtrlPtsDiff(double *state, int _start, int _end){
	for(int i=_start; i<=_end; i++){
		mSplineCR->getCtrlPt(i+1)[1] += state[mStartIndex+i];	// mCtrlPts offset by 1
	}
	if(_start==0) mSplineCR->getCtrlPt(0)[1] = mSplineCR->getCtrlPt(1)[1];
	if(_end==mNumPts-1) mSplineCR->getCtrlPt(mNumPts+1)[1] = mSplineCR->getCtrlPt(mNumPts)[1];
}

void DofSplineCatmullRom::setValue(double nFrame, double val){
	// TODO
	printf("DofSplineCatmullRom::setValue - Not yet implemented\n");
	exit(1);
}

void DofSplineCatmullRom::initValues(vector<double> q){
	// form a new spline with as many ctrl pts as frames
	int nfend = mSplineCR->getCtrlPt(mSplineCR->getNumCtrlPts()-1)[0];
	int nfstart = mSplineCR->getCtrlPt(0)[0];
	int nframes = nfend-nfstart+1;
	assert(q.size()>=nframes);
	vector<Vec2d> newctrlpts(q.size());
	for(int i=0; i<newctrlpts.size(); i++){
		newctrlpts[i][0] = nfstart+i;
		newctrlpts[i][1] = q[i];
	}
	SplineCatmullRom *newsp = new SplineCatmullRom(newctrlpts);

	// resample the spline to current mCtrlPts
	vector<double> res(mSplineCR->getNumCtrlPts());
	for(int i=0; i<res.size(); i++) {
		res[i] = mSplineCR->getCtrlPt(i)[0];
	}
	newsp->superSample(res);
	
	// delete the old spline and make the new one as the member spline
	delete mSplineCR;
	mSplineCR = newsp;
}

double DofSplineCatmullRom::getDerivative(double nFrame, int npt){
	assert(npt>=0 && npt<=3);	// only 4 pt
	return mSplineCR->getPartialDerivative(nFrame, npt);
}

double DofSplineCatmullRom::getSlopeDerivative(double nFrame, int npt){
	assert(npt>=0 && npt<=3);	// only 4 pt
	return mSplineCR->getSlopePartialDerivative(nFrame, npt);
}

double DofSplineCatmullRom::getSecondDerivative(double nFrame, int npt){
	printf("DofSplineCatmullRom::getSecondDerivative - Not yet implemented!\n");
	return 0;
}

void DofSplineCatmullRom::getCtrlPtIndices(double nFrame, int *npts, vector<int> *indices){
	indices->clear();
	*npts=4;
	int uindex = mSplineCR->getUpperBound(nFrame);
	for(int i=0; i<*npts; i++){
		int cp = uindex-2+i;
		// check if undershoots or overshoots - bring back to desired range
		if(cp==0) cp=1;
		if(cp==mNumPts+1) cp=mNumPts;
		indices->push_back(mStartIndex+cp-1);	// add dof offset and remove offset due to padding
	}
}

void DofSplineCatmullRom::getVelCtrlPtIndices(double nFrame, int *npts, vector<int> *indices){
	getCtrlPtIndices(nFrame, npts, indices);
}

void DofSplineCatmullRom::reSample(double _numPts){
	double nfstart = mSplineCR->getCtrlPt(0)[0];
	double nfend= mSplineCR->getCtrlPt(mSplineCR->getNumCtrlPts()-1)[0];

	// compute the ctrl pts
	double diff = (double)(nfend-nfstart)/(_numPts-1);
	vector<double> ctrlPts;
	ctrlPts.clear();
	ctrlPts.resize(_numPts);
	for(int i=0; i<_numPts; i++) {
		ctrlPts[i]=nfstart+i*diff;
	}
	mNumPts=_numPts;

	// pad the spline and resample
	ctrlPts.insert(ctrlPts.begin(), ctrlPts[0]);
	ctrlPts.push_back(ctrlPts[ctrlPts.size()-1]);
	mSplineCR->subSample(ctrlPts);
}

void DofSplineCatmullRom::addConstant(double _c){
	for(int i=0; i<mNumPts; i++){
		mSplineCR->getCtrlPt(i+1)[1] += _c;
	}
	mSplineCR->getCtrlPt(0)[1] = mSplineCR->getCtrlPt(1)[1];
	mSplineCR->getCtrlPt(mNumPts+1)[1] = mSplineCR->getCtrlPt(mNumPts)[1];
}
